{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sys  \n",
    "sys.path.append('../scripts/')\n",
    "from dp_policy_agent import *"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "class MeanMcl(Mcl): \n",
    "    def __init__(self, envmap, init_pose, num, motion_noise_stds={\"nn\":0.19, \"no\":0.001, \"on\":0.13, \"oo\":0.2}, \\\n",
    "                 distance_dev_rate=0.14, direction_dev=0.05):\n",
    "        super().__init__(envmap, init_pose, num, motion_noise_stds, distance_dev_rate, direction_dev)\n",
    "        \n",
    "    def normalize1(self, t): #正規化方法1（-π〜π）\n",
    "            while t < -np.pi: t += 2*np.pi\n",
    "            while t >= np.pi: t -= 2*np.pi\n",
    "            return t\n",
    "        \n",
    "    def normalize2(self, t): #正規化方法1（0〜2π）\n",
    "            while t < 0.0: t += 2*np.pi\n",
    "            while t >= 2*np.pi: t -= 2*np.pi\n",
    "            return t\n",
    "        \n",
    "    def set_mean(self): \n",
    "        x = np.array([p.pose[0] for p in self.particles]).mean()\n",
    "        y = np.array([p.pose[1] for p in self.particles]).mean()\n",
    "        \n",
    "        ts1 = np.array([self.normalize1(p.pose[2]) for p in self.particles])\n",
    "        ts2 = np.array([self.normalize2(p.pose[2]) for p in self.particles])\n",
    "        \n",
    "        t = ts1.mean() if ts1.var() < ts2.var() else ts2.mean()\n",
    "        \n",
    "        self.pose = np.array([x,y,t]).T\n",
    "            \n",
    "    def observation_update(self, observation): \n",
    "        for p in self.particles:\n",
    "            p.observation_update(observation, self.map, self.distance_dev_rate, self.direction_dev) \n",
    "        self.set_ml() #リサンプリング前に実行\n",
    "        self.resampling() \n",
    "        self.set_mean() #リサンプリング後に実行"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "def trial(animation): \n",
    "    time_interval = 0.1\n",
    "    world = PuddleWorld(30, time_interval, debug=not animation) \n",
    "\n",
    "    ##ランドマークの追加（意地悪な位置に）##\n",
    "    m = Map()\n",
    "#    for ln in [(1,4), (4,1), (-4,-4)]: m.append_landmark(Landmark(*ln))\n",
    "    for ln in [(-4,2), (2,-3), (4,4), (-4,-4)]: m.append_landmark(Landmark(*ln)) #ランドマークが十分多い場合\n",
    "    world.append(m) \n",
    "    \n",
    "    ##ゴール・水たまりの追加（これは特に変更なし）##\n",
    "    goal = Goal(-3,-3)\n",
    "    puddles = [Puddle((-2, 0), (0, 2), 0.1), Puddle((-0.5, -2), (2.5, 1), 0.1)] \n",
    "    world.append(goal)\n",
    "    world.append(puddles[0]) \n",
    "    world.append(puddles[1])\n",
    "    \n",
    "    ##ロボットを作る##\n",
    "    init_pose = np.array([2.5, 2.5, 0]).T\n",
    "    pf = MeanMcl(m, init_pose, 100)\n",
    "    a = DpPolicyAgent(time_interval, pf, goal)\n",
    "    r = Robot(init_pose, sensor=Camera(m), agent=a, color=\"red\")\n",
    "\n",
    "    world.append(r)\n",
    "        \n",
    "    world.draw()\n",
    "    \n",
    "    return a #a.total_reward+a.final_value, a.in_goal"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/usr/local/lib/python3.7/site-packages/matplotlib/pyplot.py:514: RuntimeWarning: More than 20 figures have been opened. Figures created through the pyplot interface (`matplotlib.pyplot.figure`) are retained until explicitly closed and may consume too much memory. (To control this warning, see the rcParam `figure.max_open_warning`).\n",
      "  max_open_warning, RuntimeWarning)\n"
     ]
    }
   ],
   "source": [
    "def evaluation():\n",
    "    #with open(\"avg_result.txt\", \"w\") as f:\n",
    "    with open(\"enough_landmark_result.txt\", \"w\") as f:\n",
    "        num = 1000\n",
    "        for i in range(num):\n",
    "            a = trial(False)\n",
    "            f.write(\"{} {}\\n\".format(a.total_reward+a.final_value, a.in_goal))\n",
    "            f.flush()\n",
    "            \n",
    "evaluation()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
